# -*- coding: utf-8 -*-
'''
Created on 05.09.2019

@author: yu03
'''
import matplotlib.pyplot as plt
import numpy as np
import datetime
from PyUeye import file_name, file_path, txt_name, doc_name, pico_name
import os
import matplotlib.pyplot as plt
import cv2
from scipy import signal
from FFT_Interpolation import FFT_interpolation_boxcar, FFT_interpolation_2, FFT_interpolation_compare, FFT_cal
from scipy.optimize import curve_fit
from scipy import signal
from scipy.interpolate import interp1d


def fit_cos(x, a, b, c, d):
    return a*np.cos(b*x+c) + d

def running_mean(x, N):
    cumsum = np.cumsum(np.insert(x, 0, 0)) 
#     return np.concatenate((np.zeros(N-1), (cumsum[N:] - cumsum[:-N]) / float(N)))
    return (cumsum[N:] - cumsum[:-N]) / float(N)

'''
    #读取数据 (4通道)
'''
def Read_Data_4Ch(name):
    '''
        Return Data in File (4 Channels: Data_Ch1, Data_Ch2, Data_Ch3, Data_Ch4)
        File name required (default path)
    '''
    print('Reading Data')
    with open(name,'r') as fid:
        line=''
        while line[0:4] != '----':
            line = fid.readline()
            print(line)
            if line[0:2] == 'Fs':
                p, q, m, n = line.strip().split(' ')
                Fs = float(m)
                print('Fs = %f\n'%Fs)
        out_str = fid.readlines()
    Data_Ch1, Data_Ch2, Data_Ch3, Data_Ch4 = [], [], [], []
    for line in out_str:
        a, b, c, d= line.strip().split(', ')
        Data_Ch1.append(float(a))
        Data_Ch2.append(float(b))
        Data_Ch3.append(float(c))
        Data_Ch4.append(float(d))    
    Data_Ch1 = np.array(Data_Ch1)
    Data_Ch2 = np.array(Data_Ch2)
    Data_Ch3 = np.array(Data_Ch3)
    Data_Ch4 = np.array(Data_Ch4)
    return Data_Ch1, Data_Ch2, Data_Ch3, Data_Ch4, Fs
    
def Read_Data_2Ch(name):
    '''
        Return Data in File (4 Channels: Data_Ch1, Data_Ch2)
        File name required (default path)
    '''
    print('Reading Data')
    with open(name,'r') as fid:
        line=''
        while line[0:4] != '----':
            line = fid.readline()
            print(line)
            if line[0:2] == 'Fs':
                p, q, m, n = line.strip().split(' ')
                fs_cam = float(m)
                print('fs_cam = %f\n'%fs_cam)
        out_str = fid.readlines()
    Data_Ch1, Data_Ch2 = [], []
    for line in out_str:
        a, b= line.strip().split(', ')
        Data_Ch1.append(float(a))
        Data_Ch2.append(float(b))
    Data_Ch1 = np.array(Data_Ch1)
    Data_Ch2 = np.array(Data_Ch2)
    return Data_Ch1, Data_Ch2, fs_cam

Lamda = 633e-9
pix_size = 5.3e-6
V_x, V_y, V_z = 0, 0, 0

 
'''
    #读取数据
'''

now = datetime.datetime.now()
hor_f_fit_set, hor_phase_centers, fs_cam = Read_Data_2Ch(txt_name)
hor_angle = (V_x-Lamda*hor_f_fit_set/2)*1e6 ### urad
hor_length = np.unwrap(hor_phase_centers)/4/np.pi*Lamda*1e9 ### nm

hor_angle_mean = running_mean(hor_angle, 50)
hor_length_mean = running_mean(hor_length, 50)

'''
    #读取 SmarAct数据
'''
Pico_scale = np.fromfile(pico_name, dtype='>d')
print(Pico_scale.shape)
Pico_scale = Pico_scale[(Pico_scale<-1e-200) | (Pico_scale>1e-200)]
print(Pico_scale.shape)
Pico_scale = Pico_scale*1e9 ###in nm

# hor_length = hor_length[2000:6000]
# hor_angle = hor_angle[2000:6000]
# Pico_scale = Pico_scale[2000:6000]
# hor_length = Pico_scale

if 1: ### Plotting Noise
    
    ############### linearity range
    start_cutting, stop_cutting = 0, len(hor_phase_centers)
#     start_cutting, stop_cutting = 20, 3000
    
    selected_range = np.linspace(start_cutting, stop_cutting, stop_cutting-start_cutting)
    
    ############### Plotting
     
    plt.figure(1)
    plt.gcf().set_size_inches(18,9)
    ax1 = plt.subplot(3,2,1)
    range = [0, len(hor_angle)]
    x = (np.arange(range[1]-range[0])+range[0])/fs_cam
    y = hor_angle
    plt.plot(x, y, color='blue', marker=' ')
#     plt.plot(x[49:], hor_angle_mean, 'r')
    plt.title("Horizontal Tilting")
    plt.ylabel("Horizontal Angle (urad)")
    plt.xlabel("Time (s)")
    plt.grid(which='both', axis='both')
        
    ax2 = plt.subplot(3,2,2)
    range = [0, len(hor_length)]
    x = (np.arange(range[1]-range[0])+range[0])/fs_cam
    y = hor_length
    plt.plot(x, y, color='blue', marker=' ')
    
#     plt.plot(x[49:], hor_length_mean, 'r')
    plt.title("Horizontal Phase")
    plt.ylabel("Horizontal Length (nm)]")
    plt.xlabel("Time (s)")
    plt.grid(which='both', axis='both')
    plt.plot(np.arange(len(Pico_scale))/fs_cam, Pico_scale, 'r')

    ax3 = plt.subplot(3,2,3)
    range = [0, len(hor_angle)]
    x = (np.arange(range[1]-range[0])+range[0])/fs_cam
    y = (hor_angle-np.average(hor_angle))[range[0]:range[1]]
    plt.plot(x, y, color='blue', marker=' ')
#     plt.plot(x[49:], hor_angle_mean-np.average(hor_angle_mean), 'r')
    plt.title("Horizontal Tilting")
    plt.ylabel("Horizontal Angle (urad)")
    plt.xlabel("Time (s)")
    plt.grid(which='both', axis='both')
       
    ax4 = plt.subplot(3,2,4)
    range = [0, len(hor_length)]
    x = (np.arange(range[1]-range[0])+range[0])/fs_cam
    y = (hor_length-np.average(hor_length))[range[0]:range[1]]
    plt.plot(np.arange(len(Pico_scale))/fs_cam, Pico_scale, 'r')
    plt.plot(x, y, color='blue', marker=' ')
#     plt.plot(x[49:], hor_length_mean-np.average(hor_length_mean), 'r')
    plt.title("Horizontal Length")
    plt.ylabel("Horizontal Length (nm)")
    plt.xlabel("Time (s)")
    plt.grid(which='both', axis='both')

    ax5 = plt.subplot(3,2,5)
    f_line, PS = signal.welch(hor_angle, fs_cam, nperseg=len(hor_angle))
    PSD = np.sqrt(PS)
    plt.loglog(f_line, PSD, 'b')
    plt.title("Horizontal Tilting Noise")
    plt.ylabel("PSD (urad/$\sqrt{Hz}$)")
    plt.xlabel("Freq. (Hz)")
    plt.grid(which='both', axis='both')
    plt.ylim(1e-5, 1e0) # for Ramp/20s noise
#     plt.ylim(1e-4, 0.1) # for noise
    
    ax6 = plt.subplot(3,2,6)
    f_line, PS = signal.welch(Pico_scale, fs_cam, nperseg=len(Pico_scale))
    PSD = np.sqrt(PS)
    plt.loglog(f_line, PSD, 'r')
    f_line, PS = signal.welch(hor_length, fs_cam, nperseg=len(hor_length))
    PSD = np.sqrt(PS)
    plt.loglog(f_line, PSD, 'b')
    plt.title("Horizontal Length Noise")
    plt.ylabel("PSD (nm/$\sqrt{Hz}$)")
    plt.xlabel("Freq. (Hz)")
    plt.grid(which='both', axis='both')
    plt.ylim(1e-5, 1e0) # for Ramp/20s noise
#     plt.ylim(1e-6, 2e-2) # for square wave
#     plt.ylim(1e-6, 1e-3)# for noise
    
    plt.tight_layout()
#     
#     
#     plt.figure('SmarAct')
#     f_line, PS = signal.welch(Pico_scale, fs_cam, nperseg=len(Pico_scale))
#     PSD = np.sqrt(PS)
#     plt.loglog(f_line, PSD, 'b')
#     plt.grid(which='both', axis='both')
    
    
    plt.show()




if 0: ### Plotting Sine wave
    
    ############### Plotting
     
    plt.figure(1)
    plt.gcf().set_size_inches(18,9)
    ax1 = plt.subplot(3,2,1)
    range = [0, len(hor_angle)]
    x = (np.arange(range[1]-range[0])+range[0])/fs_cam
    y = hor_angle
    hor_angle_fit = np.polyfit(x, y, 1)
    hor_angle_fitline = np.poly1d(hor_angle_fit)
    hor_angle_fitline = hor_angle_fitline(x)
    # Runs curve fitting with initial guess.
#     popt, pcov = curve_fit(fit_cos, x, y, p0=[2, 6, 1,-966])
#     # Calculates the fitted curve
#     hor_angle_fitline = fit_cos(x, *popt)
# #     print(hor_angle_fitline)
    plt.plot(x, y, color='blue', marker=' ')
    plt.plot(x, hor_angle_fitline, 'r')
    plt.title("Horizontal Tilting")
    plt.ylabel("Horizontal Angle (urad)")
    plt.xlabel("Time (s)")
    plt.grid(which='both', axis='both')
        
    ax2 = plt.subplot(3,2,2)
    range = [0, len(hor_length)]
    x = (np.arange(range[1]-range[0])+range[0])/fs_cam
    y = hor_length
    plt.plot(x, y, color='blue', marker=' ')    
    
#     params = curve_fit(fit_cos, x, hor_length)
#     [a, b, c, d] = params[0]
#     hor_length_fitline = a*np.cos(b*x+c) + d
    hor_length_fit = np.polyfit(x, y, 8)
    hor_length_fitline = np.poly1d(hor_length_fit)
    hor_length_fitline = hor_length_fitline(x)
#     # Runs curve fitting with initial guess.
#     popt, pcov = curve_fit(fit_cos, x, y, p0=[4000, 6, 1,-2000])
#     # Calculates the fitted curve
#     hor_length_fitline = fit_cos(x, *popt)
#     
    plt.plot(x, hor_length_fitline, color='red', marker=' ')
    plt.title("Horizontal Length")
    plt.ylabel("Horizontal Length (nm)")
    plt.xlabel("Time (s)")
    plt.grid(which='both', axis='both')

    ax3 = plt.subplot(3,2,3)
    range = [0, len(hor_angle)]
    x = (np.arange(range[1]-range[0])+range[0])/fs_cam
    y = hor_angle - hor_angle_fitline
    plt.plot(x, y, color='blue', marker=' ')
    plt.title("Horizontal Angle")
    plt.ylabel("Horizontal Angle (urad)")
    plt.xlabel("Time (s)")
    plt.grid(which='both', axis='both')
    

    ax4 = plt.subplot(3,2,4)
    range = [0, len(hor_length)]
    x = (np.arange(range[1]-range[0])+range[0])/fs_cam
    y = hor_length - hor_length_fitline
    plt.plot(x, y, color='blue', marker=' ')
    plt.title("Horizontal Length")
    plt.ylabel("Horizontal Length (nm)")
    plt.xlabel("Time (s)")
    plt.grid(which='both', axis='both')

    ax5 = plt.subplot(3,2,5)
    range = [0, len(hor_angle)]
    x = (np.arange(range[1]-range[0])+range[0])/fs_cam
    f_line, fft, FFT_angle_nonlinearity, phi = FFT_cal(hor_angle - hor_angle_fitline, 1/fs_cam)
    plt.loglog(f_line, FFT_angle_nonlinearity, color='blue', marker=' ')
    plt.title("Angle Spectrum")
    plt.ylabel("Amplitude (urad)")
    plt.xlabel("Freq (Hz)")
    plt.grid(which='both', axis='both')
    plt.ylim(1e-6, 1e0)

    ax6 = plt.subplot(3,2,6)
    range = [0, len(hor_length)]
    x = (np.arange(range[1]-range[0])+range[0])/fs_cam
    f_line, fft, FFT_length_nonlinearity, phi = FFT_cal((hor_length - hor_length_fitline), 1/fs_cam)
    plt.loglog(f_line, FFT_length_nonlinearity, color='blue', marker=' ')
    plt.title("Length Spectrum")
    plt.ylabel("Amplitude (nm)")
    plt.xlabel("Freq (Hz)")
    plt.grid(which='both', axis='both')
    plt.ylim(1e-5, 1e0)

    
    
    '''
        # Position based Nolinearity & SmarAct
    '''
    if 0:
#         hor_length = hor_length[1000:2000]
#         hor_angle = hor_angle[1000:2000]
#         Pico_scale = Pico_scale[1000:2000]
         
        plt.figure('Position based Nolinearity')
        
        ax1 = plt.subplot(3,2,1)
        range = [0, len(hor_length)]
        x = (np.arange(range[1]-range[0])+range[0])/fs_cam
        y = hor_length
        plt.plot(x, y, color='blue', marker=' ', label='Sensor')  
        plt.plot(x, Pico_scale, color='red', label='SmarAct')  
        plt.title("Length")
        plt.ylabel("Length (nm)")
        plt.xlabel("Time (s)")
        plt.grid(which='both', axis='both')
        plt.legend(loc='upper left')
        
        ax3 = plt.subplot(3,2,3)
        
        range = [0, len(hor_length)]
#         x = (np.arange(range[1]-range[0])+range[0])/fs_cam
        x = hor_length
        Length_nonlinearity_t = hor_length - Pico_scale
        y = Length_nonlinearity_t
        plt.plot(x, y, color='red', marker='o', markersize=2, label='Sensor-SmarAct(T-base)')
        
        spline = interp1d(x, y, kind='cubic')
        x_new = np.linspace(x[0], x[-1], num=len(x))
        Length_nonlinearity_p = spline(x_new)
        y = Length_nonlinearity_p
        plt.plot(x_new, y, color='black', marker='o', markersize=2, label='Sensor-SmarAct(P-base)') 
        
        plt.title("Length diff")
        plt.ylabel("Length / nm")
        plt.grid(which='both', axis='both')
        plt.legend(loc='upper left')
        
        ax5 = plt.subplot(3,2,5)
        range = [0, len(hor_length)]
        x = (np.arange(range[1]-range[0])+range[0])/fs_cam
        f_line, fft, FFT_nonlinearity, phi = FFT_cal(Length_nonlinearity_t, 1/fs_cam)
        plt.loglog(f_line, FFT_nonlinearity, color='blue', marker=' ', label='Sensor-SmarAct(T-base)')
         
        f_line, fft, FFT_nonlinearity, phi = FFT_cal(Length_nonlinearity_p, 1/fs_cam)
        plt.loglog(f_line, FFT_nonlinearity, color='black', marker=' ', label='Sensor-SmarAct(P-base)')
        plt.title("Length Spectrum")
        plt.ylabel("Amplitude (nm)")
        plt.xlabel("Freq (Hz)")
        plt.grid(which='both', axis='both')
        plt.legend(loc='upper left')
        
        
        ax2 = plt.subplot(3,2,2)
        range = [0, len(hor_length)]
        x = (np.arange(range[1]-range[0])+range[0])/fs_cam
        y = hor_length
        plt.plot(x, y, color='blue', marker=' ', label='Sensor')
        
#         hor_length_fit = np.polyfit(x, hor_length, 10)
#         hor_length_fitline = np.poly1d(hor_length_fit)
        y = hor_length_fitline
        plt.plot(x, y, color='black', marker=' ', label='Fitting')
        plt.title("Length")
        plt.ylabel("Length (nm)")
        plt.xlabel("Time (s)")
        plt.grid(which='both', axis='both')
        plt.legend(loc='upper left')
        

    
        ax4 = plt.subplot(3,2,4)
        range = [0, len(hor_length)]
#         x = (np.arange(range[1]-range[0])+range[0])/fs_cam
        x = hor_length
        Length_nonlinearity_t = hor_length-hor_length_fitline
        y = Length_nonlinearity_t
        plt.plot(x, y, color='blue', marker='o', markersize=2, label='Sensor-fitting(T-base)')  
        
        spline = interp1d(x, y, kind='cubic')
        x_new = np.linspace(x[0], x[-1], num=len(x))
        Length_nonlinearity_p = spline(x_new)
        y = Length_nonlinearity_p
        plt.plot(x_new, y, color='black', marker='o', markersize=2, label='Sensor-fitting(P-base)')  

        plt.title("Length diff")
        plt.ylabel("Length (nm)")
        plt.xlabel("Posittion (nm)")
        plt.grid(which='both', axis='both')
        plt.legend(loc='upper left')

        ax6 = plt.subplot(3,2,6)
        range = [0, len(hor_length)]
        x = (np.arange(range[1]-range[0])+range[0])/fs_cam
        f_line, fft, FFT_nonlinearity, phi = FFT_cal(Length_nonlinearity_t, 1/fs_cam)
        plt.loglog(f_line, FFT_nonlinearity, color='blue', marker=' ', label='Sensor-fitting(T-base)')
         
        f_line, fft, FFT_nonlinearity, phi = FFT_cal(Length_nonlinearity_p, 1/fs_cam)
        plt.loglog(f_line, FFT_nonlinearity, color='black', marker=' ', label='Sensor-fitting(P-base)')
        plt.title("Length Spectrum")
        plt.ylabel("Amplitude (nm)")
        plt.xlabel("Freq (Hz)")
        plt.grid(which='both', axis='both')
        plt.legend(loc='upper left')
        plt.ylim(1e-4, 1)
        
    plt.tight_layout()
    plt.show()

